/*NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

	Copyright (c) 2009 Jounayd Id Salah.
	All rights reserved.

	This file is part of Blacklib.

	Blacklib is free software: you can redistribute it and/or modify it under the
	terms of the GNU Lesser General Public License as published by the Free Software
	Foundation, either version 3 of the License, or (at your option) any later version.
	Blacklib is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
	without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
	PURPOSE.  See the GNU Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public License along with
	Blacklib.  If not, see <http://www.gnu.org/licenses/>.
*/
//NNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNNN

#include <string.h>
#include "maths/valid.h"
#include "maths/types.h"
#include "maths/trigo.h"

namespace black
{
	//***************************************************************
	// vec3
	//***************************************************************

	//__________________________________________________________________________
#ifdef bDEBUG
	vec3::vec3	() : x( F_NAN ), y( F_NAN ), z( F_NAN ){}
#else
	vec3::vec3	(){}
#endif

	//__________________________________________________________________________
	vec3::vec3	( const float _x, const float _y, const float _z ) : x( _x ), y( _y ), z( _z )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	vec3::vec3	( const float fVal[3] )		: x( fVal[0] ), y( fVal[1] ), z( fVal[2] )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	vec3::vec3	( const float fVal )		: x( fVal ), y( fVal ), z( fVal )
	{
		assert( black::isValid( fVal ) );
	}

	//__________________________________________________________________________
	vec3::vec3	( const vec3& v )			: x( v.x ), y( v.y ), z( v.z )
	{
		assert( isValid() );
	}

	//__________________________________________________________________________
	const vec3& vec3::operator+ () const { return *this; }

	//__________________________________________________________________________
	vec3 vec3::operator- () const { return vec3( -x, -y, -z ); }

	//__________________________________________________________________________
	vec3 vec3::operator+( const vec3& v ) const
	{
		return vec3( x+v.x, y+v.y, z+v.z );
	}

	//__________________________________________________________________________
	vec3 vec3::operator-( const vec3& v ) const
	{
		return vec3( x-v.x, y-v.y, z-v.z );
	}

	//__________________________________________________________________________
	vec3 vec3::operator*( const float fVal ) const
	{
		return vec3( x*fVal, y*fVal, z*fVal );
	}

	//__________________________________________________________________________
	vec3 vec3::operator/( const float fVal ) const
	{
		bASSERT( fVal != 0.f );
		const float fInvVal = 1.f / fVal;
		return vec3( x*fInvVal, y*fInvVal, z*fInvVal );
	}

	//__________________________________________________________________________
	vec3& vec3::operator=( const vec3& v )
	{
		x = v.x;
		y = v.y;
		z = v.z;
		// todo memcpy & test
		return *this;
	}

	//__________________________________________________________________________
	vec3& vec3::operator=( const float fVal )
	{
		x = fVal;
		y = fVal;
		z = fVal;
		// todo memset & test
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator+=( const vec3& v )
	{
		x += v.x;
		y += v.y;
		z += v.z;
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator+=( const float fVal )
	{
		x += fVal;
		y += fVal;
		z += fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator-=( const vec3& v )
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator-=( const float fVal )
	{
		x -= fVal;
		y -= fVal;
		z -= fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator*=( const float fVal )
	{
		x *= fVal;
		y *= fVal;
		z *= fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec3& vec3::operator/=( const float fVal )
	{
		bASSERT( fVal != 0.f );
		const float fInvVal = 1.f / fVal;
		x *= fInvVal;
		y *= fInvVal;
		z *= fInvVal;
		return *this;
	}

	//__________________________________________________________________________
	float vec3::operator[] ( const size_t i ) const
	{
		assert( i < 3 );
		return *(&x+i);
	}

	//__________________________________________________________________________
	float& vec3::operator[] ( const size_t i )
	{
		assert( i < 3 );
		return *(&x+i);
	}

	//__________________________________________________________________________
	bool vec3::operator==( const float fVal ) const
	{
		return ( x == fVal && y == fVal && z == fVal );
	}

	//__________________________________________________________________________
	bool vec3::operator==( const vec3& v ) const
	{
		return ( x == v.x && y == v.y && z == v.z );
	}

	//__________________________________________________________________________
	bool vec3::operator!=( const float fVal ) const
	{
		return ( x != fVal || y != fVal || z != fVal );
	}

	//__________________________________________________________________________
	bool vec3::operator!=( const vec3& v ) const
	{
		return ( x != v.x || y != v.y || z != v.z );
	}

	//__________________________________________________________________________
	void vec3::set ( float fX, float fY, float fZ )
	{
		x = fX; y = fY; z = fZ;
	}

	//__________________________________________________________________________
	void vec3::set ( const vec3& v )
	{
		memcpy( this, &v, 3*sizeof( float ) );
	}

	//__________________________________________________________________________
	void vec3::set ( const vec4& v )
	{
		memcpy( this, &v, 3*sizeof( float ) );
	}

	//__________________________________________________________________________
	void vec3::setLength( const float fLength )
	{
		const float fOldLength = sqrt( x*x + y*y + z*z );
		assert( fOldLength != 0.f );
		assert( fLength >= 0.f );
		const float fCoef = fLength / fOldLength;
		x *= fCoef;
		y *= fCoef;
		z *= fCoef;
	}

	//__________________________________________________________________________
	float vec3::length () const								{ return sqrt( x*x + y*y + z*z ); }

	//__________________________________________________________________________
	float vec3::fastLength() const							{ return length(); }

	//__________________________________________________________________________
	float vec3::squaredLength () const						{ return x*x + y*y + z*z; }

	//__________________________________________________________________________
	float vec3::distance ( const vec3& v ) const			{ return sqrt( v.x*x + v.y*y + v.z*z ); }

	//__________________________________________________________________________
	float vec3::squaredDistance ( const vec3& v ) const		{ return v.x*x + v.y*y + v.z*z; }

	//__________________________________________________________________________
	float vec3::dot ( const vec3& v ) const					{ return x*v.x + y*v.y + z*v.z; }

	//__________________________________________________________________________
	vec3 vec3::cross ( const vec3& v ) const				{ return vec3( y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x ); }

	//__________________________________________________________________________
	void vec3::cross ( vec3& res, const vec3& v ) const		{ res = vec3( y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x ); }

	//__________________________________________________________________________
	void vec3::normalize()
	{
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = 1.f / sqrt( fSquareLength );
		x *= fInvSqrt;
		y *= fInvSqrt;
		z *= fInvSqrt;
	}

	//__________________________________________________________________________
	vec3 vec3::normalized() const
	{
		vec3 v( *this );
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = 1.f / sqrt( fSquareLength );
		v.x *= fInvSqrt;
		v.y *= fInvSqrt;
		v.z *= fInvSqrt;
		return v;
	}

	//__________________________________________________________________________
	void vec3::normalizeFast()
	{
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = fastInvSqrt( fSquareLength );
		x *= fInvSqrt;
		y *= fInvSqrt;
		z *= fInvSqrt;
	}

	//__________________________________________________________________________
	bool vec3::isEqual( const vec3& v, float epsilon ) const
	{
		bASSERT( epsilon >= 0.f );
		return abs( v.x-x ) < epsilon
			&& abs( v.y-y ) < epsilon
			&& abs( v.z-z ) < epsilon;
	}

	//__________________________________________________________________________
	bool vec3::isEqual( const float fVal, float epsilon ) const
	{
		bASSERT( epsilon >= 0.f );
		return abs( fVal-x ) < epsilon
			&& abs( fVal-y ) < epsilon
			&& abs( fVal-z ) < epsilon;
	}

	//__________________________________________________________________________
	void vec3::setZero()
	{
		memcpy( this, &vec3::ZERO, 12 );
	}

	//__________________________________________________________________________
	bool vec3::isZero() const
	{
		return x == 0.f && y == 0.f && z == 0.f;
	}

	//__________________________________________________________________________
	bool vec3::isNearZero() const
	{
		return abs( x ) < fEPSILON && abs( y ) < fEPSILON && abs( z ) < fEPSILON;
	}

	//__________________________________________________________________________
	bool vec3::isUnit() const
	{
		return x*x+y*y+z*z == 1.f;
	}

	//__________________________________________________________________________
	bool vec3::isNearUnit() const
	{
		return x*x+y*y+z*z < ( fEPSILON*fEPSILON + 1.f );
	}

	//__________________________________________________________________________
	void vec3::clamp ( const float fMin, const float fMax )
	{
		x = x < fMin ? fMin : x;
		y = y < fMin ? fMin : y;
		z = z < fMin ? fMin : z;

		x = x > fMax ? fMax : x;
		y = y > fMax ? fMax : y;
		z = z > fMax ? fMax : z;
	}

	//__________________________________________________________________________
	bool vec3::isValid() const
	{
		return black::isValid( x ) && black::isValid( y ) && black::isValid( z );
	}

	//__________________________________________________________________________
	void vec3::lerp( const vec3& v, const float fCoef )
	{
		x = x + fCoef * ( v.x - x );
		y = y + fCoef * ( v.y - y );
		z = z + fCoef * ( v.z - z );
	}

	//__________________________________________________________________________
	void vec3::nlerp( const vec3& v, float fCoef )
	{
		assert( isNearUnit() );
		assert( v.isNearUnit() );
		x = x + fCoef * ( v.x - x );
		y = y + fCoef * ( v.y - y );
		z = z + fCoef * ( v.z - z );
		normalizeFast();
	}

	//__________________________________________________________________________
	float vec3::angle( const vec3& v ) const
	{
		const float fCos = dot( v );
		if ( fCos >= 1.f )
			return 0.f;
		else if ( fCos <= -1.f )
			return F_PI;
		else
			return acos( fCos );
	}

	//***************************************************************
	// vec3_128
	//***************************************************************

#ifdef bSSE

	//__________________________________________________________________________
	vec3_128::vec3_128 () : vec3(), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128::vec3_128 ( const float x, const float y, const float z ) : vec3( x, y, z ), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128::vec3_128 ( const float fVal[3] ) : vec3( fVal[0], fVal[1], fVal[2] ), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128::vec3_128 ( const float fVal ) : vec3( fVal ), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128::vec3_128 ( const vec3_128& v ) : vec3( v.x, v.y, v.z ), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128::vec3_128 ( const vec3& v ) : vec3( v ), pad( 0.f ) {}

	//__________________________________________________________________________
	vec3_128& vec3_128::operator=( const float fVal )
	{
		assertAligned16Ptr( this );
		const __m128 mmV	= _mm_set_ps1( fVal );
		_mm_store_ps( &x, mmV );

		return *this;
	}

	//__________________________________________________________________________
	vec3_128& vec3_128::operator=( const vec3& v )
	{
		/*const __m128 mmV	= _mm_loadu_ps( &v.x );
		_mm_store_ps( &x, mmV );*/
		x = v.x;
		y = v.y;
		z = v.z;

		return *this;
	}

	//__________________________________________________________________________
	vec3_128& vec3_128::operator=( const vec3_128& v )
	{
		/*const __m128 mmV	= _mm_load_ps( &v.x );
		_mm_store_ps( &x, mmV );*/
		x = v.x;
		y = v.y;
		z = v.z;

		return *this;
	}

	//__________________________________________________________________________
	vec3_128& vec3_128::operator=( const __m128& v )
	{
		assertAligned16Ptr( this );
		assertAligned16Ptr( &v );
		(__m128&)*this = v;
		// todo memcpy & test
		return *this;
	}

	//__________________________________________________________________________
	const vec3_128& vec3_128::operator+=( const vec3& v )
	{
		/* can be really ultra slow sometimes due to unaligned data

		const __m128	mmV		= _mm_loadu_ps( &v.x );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_add_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );*/

		x += v.x;
		y += v.y;
		z += v.z;

		return *this;
	}

	//__________________________________________________________________________
	// 16% faster than vec3
	const vec3_128& vec3_128::operator+=( const vec3_128& v )
	{
		assertAligned16Ptr( this );
		assertAligned16Ptr( &v );
		const __m128	mmV		= _mm_load_ps( &v.x );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_add_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	// 16% faster than vec3
	const vec3_128& vec3_128::operator+=( const float fVal )
	{
		assertAligned16Ptr( this );
		const __m128	mmV		= _mm_set_ps1( fVal );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_add_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	const vec3_128& vec3_128::operator-=( const vec3& v )
	{
		/* can be really ultra slow sometimes due to unaligned data

		const __m128	mmV		= _mm_loadu_ps( &v.x );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_sub_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );*/

		x -= v.x;
		y -= v.y;
		z -= v.z;

		return *this;
	}

	//__________________________________________________________________________
	// 16% faster than vec3
	const vec3_128& vec3_128::operator-=( const vec3_128& v )
	{
		assertAligned16Ptr( this );
		assertAligned16Ptr( &v );
		const __m128	mmV		= _mm_load_ps( &v.x );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_sub_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	// 16% faster than vec3
	const vec3_128& vec3_128::operator-=( const float fVal )
	{
		assertAligned16Ptr( this );
		const __m128	mmV		= _mm_set_ps1( fVal );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_sub_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	// 20% faster than vec3
	const vec3_128& vec3_128::operator*=( const float fVal )
	{
		assertAligned16Ptr( this );
		const __m128	mmV		= _mm_set_ps1( fVal );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_mul_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	// 21% faster than vec3
	const vec3_128& vec3_128::operator/=( const float fVal )
	{
		assertAligned16Ptr( this );
		bASSERT( fVal != 0.f );
		const float fInvVal = 1.f / fVal; // precomputing division on fpu is faster
		const __m128	mmV		= _mm_set_ps1( fInvVal );
		__m128			mmThis	= _mm_load_ps( &x );

		mmThis = _mm_mul_ps( mmThis, mmV );
		_mm_store_ps( &x, mmThis );

		return *this;
	}

	//__________________________________________________________________________
	// 13% slower than vec3
	bool vec3_128::operator==( const float fVal ) const
	{
		/*slow
		const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_set_ps1( fVal );

		const __m128 mmRes	= _mm_cmpeq_ps( mmThis, mmV );
		return ( mmRes.m128_u32[ 0 ] && mmRes.m128_u32[ 1 ] && mmRes.m128_u32[ 2 ] );*/

		return ( x == fVal && y == fVal && z == fVal );
	}

	//__________________________________________________________________________
	// 4% slower than vec3
	bool vec3_128::operator==( const vec3& v ) const
	{
		/* slow
		const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_loadu_ps( &v.x );

		const __m128 mmRes	= _mm_cmpeq_ps( mmThis, mmV );
		return ( mmRes.m128_u32[ 0 ] && mmRes.m128_u32[ 1 ] && mmRes.m128_u32[ 2 ] );*/
		return ( x == v.x && y == v.y && z == v.z );
	}

	//__________________________________________________________________________
	bool vec3_128::operator==( const vec3_128& v ) const
	{
		//bASSERT( pad == v.pad );
		/*const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_load_ps( &v.x );

		const __m128 mmRes	= _mm_cmpeq_ps( mmThis, mmV );
		return ( mmRes.m128_u32[ 0 ] && mmRes.m128_u32[ 1 ] && mmRes.m128_u32[ 2 ] );*/
		return ( x == v.x && y == v.y && z == v.z );
	}

	//__________________________________________________________________________
	// 13% slower than vec3
	bool vec3_128::operator!=( const float fVal ) const
	{
		/* slow
		const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_set_ps1( fVal );

		const __m128 mmRes	= _mm_cmpneq_ps( mmThis, mmV );

		return ( mmRes.m128_u32[ 0 ] || mmRes.m128_u32[ 1 ] || mmRes.m128_u32[ 2 ] );*/
		return ( x != fVal || y != fVal || z != fVal );
	}

	//__________________________________________________________________________
	// 4% slower than vec3
	bool vec3_128::operator!=( const vec3& v ) const
	{
		/* 10% slower than vec3
		const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_loadu_ps( &v.x );

		const __m128 mmRes	= _mm_cmpneq_ps( mmThis, mmV );

		return ( mmRes.m128_u32[ 0 ] || mmRes.m128_u32[ 1 ] || mmRes.m128_u32[ 2 ] );
		*/

		return ( x != v.x || y != v.y || z != v.z );
	}

	//__________________________________________________________________________
	bool vec3_128::operator!=( const vec3_128& v ) const
	{
		/*const __m128 mmThis	= _mm_load_ps( &x );
		const __m128 mmV	= _mm_load_ps( &v.x );

		const __m128 mmRes	= _mm_cmpneq_ps( mmThis, mmV );

		return ( mmRes.m128_u32[ 0 ] || mmRes.m128_u32[ 1 ] || mmRes.m128_u32[ 2 ] );*/

		return ( x != v.x || y != v.y || z != v.z );
	}

	//__________________________________________________________________________
	float vec3_128::length() const
	{
#ifdef bSSE3
		data128 mmTmp;
		mmTmp.m128	= _mm_mul_ps( *this, *this );
		mmTmp.m128	= _mm_hadd_ps( mmTmp.m128, mmTmp.m128 );
		mmTmp.m128	= _mm_hadd_ps( mmTmp.m128, mmTmp.m128 );
		mmTmp.m128	= _mm_sqrt_ps( mmTmp.m128 );
		return mmTmp.f[ 0 ];
#else
		data128 mmTmp;
		mmTmp.m128 = _mm_mul_ps( *this, *this );
		const float fSquareLength = mmTmp.f[ 0 ] + mmTmp.f[ 1 ] + mmTmp.f[ 2 ];
		bASSERT( fSquareLength >= 0.f );
		return sqrt( fSquareLength );
#endif
	}

	//__________________________________________________________________________
	// 2% slower than vec3
	float vec3_128::squaredLength() const
	{
		/* slow
		__m128	mmTmp = _mm_load_ps( &x );

		mmTmp = _mm_mul_ps( mmTmp, mmTmp );
		return mmTmp.m128_f32[ 0 ] + mmTmp.m128_f32[ 1 ] + mmTmp.m128_f32[ 2 ];*/

		return dot( *this );
	}

	//__________________________________________________________________________
	float vec3_128::dot( const vec3_128& v ) const
	{
		/* 30% slower than vec3
		__m128			mmTmp	= _mm_load_ps( &x );
		const __m128	mmTmp2	= _mm_load_ps( &v.x );

		mmTmp = _mm_mul_ps( mmTmp, mmTmp2 );
		return mmTmp.m128_f32[ 0 ] + mmTmp.m128_f32[ 1 ] + mmTmp.m128_f32[ 2 ];*/
#ifdef bSSE3
		data128 mmTmp;
		mmTmp.m128	= _mm_mul_ps( *this, v );
		mmTmp.m128	= _mm_hadd_ps( mmTmp.m128, mmTmp.m128 );
		mmTmp.m128	= _mm_hadd_ps( mmTmp.m128, mmTmp.m128 );
		return mmTmp.f[ 0 ];
#else
		/*data128 mmMul;
		mmMul.m128 = _mm_mul_ps( *this, v );
		return mmMul.f[ 0 ] + mmMul.f[ 1 ] + mmMul.f[ 2 ];*/
		return x*v.x + y*v.y + z*v.z;
#endif //bSSE3
	}

	//__________________________________________________________________________
	__m128 vec3_128::dot( const vec3_128 v[4] ) const
	{
#ifdef bSSE3
		const __m128 mmMul0		= _mm_mul_ps( *this, v[0] );
		const __m128 mmMul1		= _mm_mul_ps( *this, v[1] );
		const __m128 mmMul2		= _mm_mul_ps( *this, v[2] );
		const __m128 mmMul3		= _mm_mul_ps( *this, v[3] );

		const __m128 mmHSum01	= _mm_hadd_ps( mmMul0, mmMul1 );
		const __m128 mmHSum23	= _mm_hadd_ps( mmMul2, mmMul3 );
		const __m128 mmRes		= _mm_hadd_ps( mmHSum01, mmHSum23 );

		return mmRes;
#else
		data128 mmMul0, mmMul1, mmMul2, mmMul3;
		mmMul0.m128 = _mm_mul_ps( *this, v[0] );
		mmMul1.m128 = _mm_mul_ps( *this, v[1] );
		mmMul2.m128 = _mm_mul_ps( *this, v[2] );
		mmMul3.m128 = _mm_mul_ps( *this, v[3] );
		data128 mmRes;
		mmRes.f[0] = mmMul0.f[0] + mmMul0.f[1];
		mmRes.f[1] = mmMul1.f[0] + mmMul1.f[1];
		mmRes.f[2] = mmMul2.f[0] + mmMul2.f[1];
		mmRes.f[3] = mmMul3.f[0] + mmMul3.f[1];
		mmRes.f[0] += mmMul0.f[2];
		mmRes.f[1] += mmMul1.f[2];
		mmRes.f[2] += mmMul2.f[2];
		mmRes.f[3] += mmMul2.f[2];
		return mmRes.m128;
#endif
	}

	//__________________________________________________________________________
	void vec3_128::cross ( vec3_128& vRes, const vec3_128& v ) const
	{
		const __m128 mmShufThis1	= _mm_shuffle_ps(bConstM128(*this), bConstM128(*this), _MM_SHUFFLE(3, 1, 2, 0) );
		const __m128 mmShufV1		= _mm_shuffle_ps(bConstM128(v), bConstM128(v), _MM_SHUFFLE(3, 2, 0, 1) );

		const __m128 mmShufThis2	= _mm_shuffle_ps(bConstM128(*this), bConstM128(*this), _MM_SHUFFLE(3, 2, 0, 1) );
		const __m128 mmShufV2		= _mm_shuffle_ps(bConstM128(v), bConstM128(v), _MM_SHUFFLE(3, 1, 2, 0) );

		__m128 mmTmp1 = _mm_mul_ps( mmShufThis1, mmShufV1 );
		__m128 mmTmp2 = _mm_mul_ps( mmShufV2, mmShufThis2 );

		vRes = _mm_sub_ps( mmTmp1, mmTmp2 );
	}

	//__________________________________________________________________________
	void vec3_128::normalize()
	{
#ifdef bSSE3
		__m128 mmTmp;
		mmTmp		= _mm_mul_ps( *this, *this );
		mmTmp		= _mm_hadd_ps( mmTmp, mmTmp );
		mmTmp		= _mm_hadd_ps( mmTmp, mmTmp );
		mmTmp		= _mm_rsqrt_ps( mmTmp );
		*this		= _mm_mul_ps( *this, mmTmp );
#else
		data128 mmTmp;
		mmTmp.m128 = _mm_mul_ps( *this, *this );
		const float fSquareLength = mmTmp.f[0] + mmTmp.f[1] + mmTmp.f[2];
		bASSERT( fSquareLength > 0.f );
		mmTmp		= _mm_set_ps1( fSquareLength );
		mmTmp		= _mm_rsqrt_ps( mmTmp );
		*this		= _mm_mul_ps( *this, mmTmp );
#endif
	}

	//__________________________________________________________________________
	vec3_128 vec3_128::normalized() const
	{
		vec3_128 vRes( *this );
		vRes.normalize();

		return vRes;
	}

	//__________________________________________________________________________
	void vec3_128::normalizeFast()
	{
#ifdef bCOMPILER_AUTO_SSE
		normalize();
#else
		__m128 mmThis = _mm_load_ps( &x );

		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = fastInvSqrt( fSquareLength );

		const __m128 mmInvSqrt = _mm_set_ps1( fInvSqrt );
		mmThis = _mm_mul_ps( mmThis, mmInvSqrt );
		_mm_store_ps( &this->x, mmThis );
#endif
	}

	//__________________________________________________________________________
	// 4% faster than vec3
	void vec3_128::setZero()
	{
#ifdef bSSE
		*( (__m128*)this ) = _mm_setzero_ps();
#else
		memcpy( this, &vec3::ZERO, 12 );
#endif

		//*this = vec3::ZERO;
		/*static const __m128 mmZero = _mm_setzero_ps();
		_mm_store_ps( &this->x, mmZero );*/
	}

	//__________________________________________________________________________
	vec3_128::operator __m128&() { return reinterpret_cast<__m128&>( *this ); }

	//__________________________________________________________________________
	vec3_128::operator const __m128&() const { return reinterpret_cast<const __m128&>( *this ); }

#endif // bSSE

	//***************************************************************
	// vec4
	//***************************************************************

	//__________________________________________________________________________
	vec4::vec4 () : w( 1.f ) {}

	//__________________________________________________________________________
	vec4::vec4 ( const float _x, const float _y, const float _z, const float _w ) : vec3( _x, _y, _z ), w( _w ) {}

	//__________________________________________________________________________
	vec4::vec4 ( const float fVal[4] ) : vec3( fVal[0], fVal[1], fVal[2] ), w( fVal[3] ) {}

	//__________________________________________________________________________
	vec4::vec4 ( const float fVal ) : vec3( fVal ), w( 1.f ) {}

	//__________________________________________________________________________
	vec4::vec4 ( const vec4& v ) : vec3( v ), w( v.w ) {}

	//__________________________________________________________________________
	const vec4& vec4::operator+ () const { return *this; }

	//__________________________________________________________________________
	vec4 vec4::operator- () const { return vec4( -x, -y, -z, 1.f ); }

	//__________________________________________________________________________
	vec4 vec4::operator+( const vec4& v ) const
	{
		return vec4( x+v.x, y+v.y, z+v.z, 1.f );
	}

	//__________________________________________________________________________
	vec4 vec4::operator-( const vec4& v ) const
	{
		return vec4( x-v.x, y-v.y, z-v.z, 1.f );
	}

	//__________________________________________________________________________
	vec4 vec4::operator*( const float fVal ) const
	{
		return vec4( x*fVal, y*fVal, z*fVal, 1.f );
	}

	//__________________________________________________________________________
	vec4 vec4::operator/( const float fVal ) const
	{
		bASSERT( fVal != 0.f );
		const float fInvVal = 1.f / fVal;
		return vec4( x*fInvVal, y*fInvVal, z*fInvVal, 1.f );
	}

	//__________________________________________________________________________
	vec4& vec4::operator=( const vec4& v )
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = v.w;
		// todo memcpy & test
		return *this;
	}

	//__________________________________________________________________________
	vec4& vec4::operator=( const float fVal )
	{
		x = fVal;
		y = fVal;
		z = fVal;
		// todo memset & test
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator+=( const vec4& v )
	{
		x += v.x;
		y += v.y;
		z += v.z;
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator+=( const float fVal )
	{
		x += fVal;
		y += fVal;
		z += fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator-=( const vec4& v )
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator-=( const float fVal )
	{
		x -= fVal;
		y -= fVal;
		z -= fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator*=( const float fVal )
	{
		x *= fVal;
		y *= fVal;
		z *= fVal;
		return *this;
	}

	//__________________________________________________________________________
	const vec4& vec4::operator/=( const float fVal )
	{
		bASSERT( fVal != 0.f );
		const float fInvVal = 1.f / fVal;
		x *= fInvVal;
		y *= fInvVal;
		z *= fInvVal;
		return *this;
	}

	//__________________________________________________________________________
	float vec4::operator [] ( const size_t i ) const { return *(&x+i); }

	//__________________________________________________________________________
	float& vec4::operator [] ( const size_t i ) { return *(&x+i); }

	//__________________________________________________________________________
	bool vec4::operator==( const float fVal ) const
	{
		return ( x == fVal && y == fVal && z == fVal && w == 1.f );
	}

	//__________________________________________________________________________
	bool vec4::operator==( const vec4& v ) const
	{
		return ( x == v.x && y == v.y && z == v.z && w == v.w );
	}

	//__________________________________________________________________________
	bool vec4::operator!=( const float fVal ) const
	{
		return ( x != fVal || y != fVal || z != fVal || w != 1.f );
	}

	//__________________________________________________________________________
	bool vec4::operator!=( const vec4& v ) const
	{
		return ( x != v.x || y != v.y || z != v.z || w != v.w );
	}

	//__________________________________________________________________________
	void vec4::set ( float fX, float fY, float fZ, float fW )
	{
		x = fX; y = fY; z = fZ; w = fW;
	}

	//__________________________________________________________________________
	void vec4::set ( const vec3& v )
	{
		memcpy( this, &v, 3*sizeof( float ) );
		w = 1.f;
	}

	//__________________________________________________________________________
	void vec4::set ( const vec4& v )
	{
		memcpy( this, &v, 4*sizeof( float ) );
	}

	//__________________________________________________________________________
	float vec4::length () const { return sqrt( x*x + y*y + z*z + w*w ); }

	//__________________________________________________________________________
	float vec4::squaredLength () const { return x*x + y*y + z*z + w*w; }

	//__________________________________________________________________________
	float vec4::dot ( const vec4& v ) const { return x*v.x + y*v.y + z*v.z + w*v.w; }

	//__________________________________________________________________________
	vec4 vec4::cross ( const vec4& v ) const { return vec4( y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x, 1.f ); }

	//__________________________________________________________________________
	void vec4::cross ( vec4& res, const vec4& v ) const { res = vec4( y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x, 1.f ); }

	//__________________________________________________________________________
	void vec4::normalize()
	{
		// todo
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = 1.f / sqrt( fSquareLength );
		x *= fInvSqrt;
		y *= fInvSqrt;
		z *= fInvSqrt;
	}

	//__________________________________________________________________________
	vec4 vec4::normalized() const
	{
		// todo
		vec4 v( *this );
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = 1.f / sqrt( fSquareLength );
		v.x *= fInvSqrt;
		v.y *= fInvSqrt;
		v.z *= fInvSqrt;
		return v;
	}

	//__________________________________________________________________________
	void vec4::normalizeFast()
	{
		// todo
		const float fSquareLength = x*x + y*y + z*z;
		bASSERT( fSquareLength != 0.f );
		const float fInvSqrt = fastInvSqrt( fSquareLength );
		x *= fInvSqrt;
		y *= fInvSqrt;
		z *= fInvSqrt;
	}

	//__________________________________________________________________________
	bool vec4::isEqual( const vec4& v, const float epsilon ) const
	{
		bASSERT( epsilon >= 0.f );
		return abs( v.x-x ) < epsilon
			&& abs( v.y-y ) < epsilon
			&& abs( v.z-z ) < epsilon
			&& abs( v.w-w ) < epsilon;
	}

	//__________________________________________________________________________
	bool vec4::isEqual( const float fVal, const float epsilon ) const
	{
		bASSERT( epsilon >= 0.f );
		return abs( fVal-x ) < epsilon
			&& abs( fVal-y ) < epsilon
			&& abs( fVal-z ) < epsilon
			&& abs( 1.f-w ) < epsilon;
	}

	//__________________________________________________________________________
	void vec4::setZero()
	{
		memcpy( this, &vec4::ZERO, 16 );
	}

	//__________________________________________________________________________
	bool vec4::isValid() const
	{
		return black::isValid( x ) && black::isValid( y ) && black::isValid( z ) && black::isValid( w );
	}

	//__________________________________________________________________________
	bool vec4::isUnit() const
	{
		return x*x+y*y+z*z+w*w < ( fEPSILON*fEPSILON + 1.f );
	}

	//__________________________________________________________________________
	void vec4::lerp( const vec4& v, const float fCoef )
	{
		x = ( v.x - x ) * fCoef + x;
		y = ( v.y - y ) * fCoef + y;
		z = ( v.z - z ) * fCoef + z;
		w = ( v.w - w ) * fCoef + w;
	}

	//***************************************************************
	// vec4_128
	//***************************************************************

	//__________________________________________________________________________
	vec4_128::vec4_128 () {}

	//__________________________________________________________________________
	vec4_128::vec4_128 ( const float _x, const float _y, const float _z, const float _w ) : vec4( _x, _y, _z, _w ) {}

	//__________________________________________________________________________
	vec4_128::vec4_128 ( const float fVal[4] ) : vec4( fVal ) {}

	//__________________________________________________________________________
	vec4_128::vec4_128 ( const float fVal ) : vec4( fVal ) {}

	//__________________________________________________________________________
	vec4_128::vec4_128 ( const vec4& v ) : vec4( v ) {}

	//__________________________________________________________________________
	vec4_128::operator __m128&()
	{
		return reinterpret_cast<__m128&>( x );
	}

	//__________________________________________________________________________
	vec4_128::operator const __m128&() const
	{
		return reinterpret_cast<const __m128&>( x );
	}

} // namespace black
